/*
 * Copyright (c) 2021 Talkweb Co., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include "cmsis_os2.h"
#include "gyro.h"
#include "iot_errno.h"
#include "iot_gpio.h"
#include "iot_i2c.h"

#define I2C0_INDEX 0

static void MPUIsrFunc(char *arg)
{
    printf("usr key pressed %s\n", arg);
    return;
}

static void GyroIOInit(void)
{
    IoTGpioInit(IOT_GPIO_IO_GPIO_7);
    IoTIoSetFunc(IOT_GPIO_IO_GPIO_7, IOT_IO_FUNC_GPIO_7_GPIO); //设置GPIO_7的复用功能为普通GPIO
    IoTGpioSetDir(IOT_GPIO_IO_GPIO_7, IOT_GPIO_DIR_OUT);       //设置GPIO_7为输出模式

    IoTGpioInit(IOT_GPIO_IO_GPIO_8);
    IoTIoSetFunc(IOT_GPIO_IO_GPIO_8, IOT_IO_FUNC_GPIO_8_GPIO); //设置GPIO_8的复用功能为普通GPIO
    IoTGpioSetDir(IOT_GPIO_IO_GPIO_8, IOT_GPIO_DIR_OUT);       //设置GPIO_8为输出模式

    IoTGpioInit(IOT_GPIO_IO_GPIO_12);
    IoTIoSetFunc(IOT_GPIO_IO_GPIO_12, IOT_IO_FUNC_GPIO_12_GPIO); //设置GPIO_8的复用功能为普通GPIO
    IoTGpioSetDir(IOT_GPIO_IO_GPIO_12, IOT_GPIO_DIR_IN);         //设置GPIO_8为输出模式
    IoTIoSetPull(IOT_GPIO_IO_GPIO_12, IOT_IO_PULL_UP);
    IoTGpioRegisterIsrFunc(IOT_GPIO_IO_GPIO_12, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_RISE_LEVEL_HIGH, MPUIsrFunc, NULL);

    IoTGpioInit(IOT_GPIO_IO_GPIO_13);
    IoTGpioInit(IOT_GPIO_IO_GPIO_14);
    IoTIoSetFunc(IOT_GPIO_IO_GPIO_13, IOT_IO_FUNC_GPIO_13_I2C0_SDA);
    IoTIoSetFunc(IOT_GPIO_IO_GPIO_14, IOT_IO_FUNC_GPIO_14_I2C0_SCL);

    IoTI2cInit(I2C0_INDEX, 400000); /* baudrate: 400kbps */
    IoTI2cSetBaudrate(I2C0_INDEX, 400000);
}

static void I2CMPU6050WriteData(uint8_t Reg, uint8_t Value)
{
    uint8_t send_data[2] = {Reg, Value};
    IoTI2cWrite(I2C0_INDEX, (MPU6050_SLAVE_ADDRESS << 1) | 0x00, send_data, 2);
}

static uint8_t I2CMPU6050WriteBuffer(uint8_t Reg, uint8_t *pBuffer, uint16_t Length)
{
    uint32_t status = 0;
    uint8_t send_data[256] = {0};

    send_data[0] = Reg;
    for (int j = 0; j < Length; j++)
    {
        send_data[j + 1] = pBuffer[j];
    }

    status = IoTI2cWrite(I2C0_INDEX, (MPU6050_SLAVE_ADDRESS << 1) | 0x00, send_data, Length + 1);
    if (status != 0)
    {
        printf("===== Error: I2C write status1 = 0x%x! =====\r\n", status);
        return status;
    }

    return 0;
}

static uint8_t I2CMPU6050ReadData(uint8_t Reg)
{
    uint8_t value = 0;
    uint32_t status = 0;
    uint8_t buffer[1] = {Reg};
    IotI2cData wData = {0};
    wData.sendBuf = buffer;
    wData.sendLen = 1;
    wData.receiveBuf = &value;
    wData.receiveLen = 1;
    status = IoTI2cWriteread(I2C0_INDEX, (MPU6050_SLAVE_ADDRESS << 1) | 0x00, &wData);
    if (status != 0)
    {
        printf("===== Error: I2C write status = 0x%x! =====\r\n", status);
        return status;
    }
    return value;
}

uint8_t I2CMPU6050ReadBuffer(uint8_t Reg, uint8_t *pBuffer, uint16_t Length)
{

    uint32_t status = 0;
    uint8_t buffer[1] = {Reg};
    IotI2cData wData = {0};
    wData.sendBuf = buffer;
    wData.sendLen = 1;
    wData.receiveBuf = pBuffer;
    wData.receiveLen = Length;
    status = IoTI2cWriteread(I2C0_INDEX, (MPU6050_SLAVE_ADDRESS << 1) | 0x00, &wData);
    if (status != 0)
    {
        printf("===== Error: I2C write status2.0 = 0x%x! =====\r\n", status);
        return status;
    }

    return 0;
}

void MPU6050WriteReg(uint8_t reg_add, uint8_t reg_dat)
{
    I2CMPU6050WriteData(reg_add, reg_dat);
}

void MPU6050ReadData(uint8_t reg_add, unsigned char *Read, uint8_t num)
{
    I2CMPU6050ReadBuffer(reg_add, Read, num);
}

void MPU6050ReadAcc(short *accData)
{
    uint8_t buf[6];
    MPU6050ReadData(MPU6050_ACC_OUT, buf, 6);
    accData[0] = (buf[0] << 8) | buf[1];
    accData[1] = (buf[2] << 8) | buf[3];
    accData[2] = (buf[4] << 8) | buf[5];
    printf("MPU6050ReadAcc accData[0] ==%d\t accData[1] ==%d\t accData[2] ==%d\n", accData[0], accData[1], accData[2]);
}

void MPU6050ReadGyro(short *gyroData)
{
    uint8_t buf[6];
    MPU6050ReadData(MPU6050_GYRO_OUT, buf, 6);
    gyroData[0] = (buf[0] << 8) | buf[1];
    gyroData[1] = (buf[2] << 8) | buf[3];
    gyroData[2] = (buf[4] << 8) | buf[5];
    printf("MPU6050ReadAcc gyroData[0] ==%d\t gyroData[1] ==%d\t gyroData[2] ==%d\n", gyroData[0], gyroData[1], gyroData[2]);
}

void MPU6050ReadTemp(short *tempData)
{
    uint8_t buf[2];
    MPU6050ReadData(MPU6050_RA_TEMP_OUT_H, buf, 2); //读取温度值
    *tempData = (buf[0] << 8) | buf[1];
}

void MPU6050ReturnTemp(short *Temperature)
{
    short temp3;
    uint8_t buf[2];

    MPU6050ReadData(MPU6050_RA_TEMP_OUT_H, buf, 2); //读取温度值
    temp3 = (buf[0] << 8) | buf[1];
    *Temperature = (((double)(temp3 + 13200)) / 280) - 13;
    printf("MPU6050ReturnTemp = %d/n", *Temperature);
}

void FreeFallInterrupt(void) //自由落体中断
{
    MPU6050WriteReg(MPU6050_RA_FF_THR, 0x01); //自由落体阈值
    MPU6050WriteReg(MPU6050_RA_FF_DUR, 0x01); //自由落体检测时间20ms 单位1ms 寄存器0X20
}
void MotionInterrupt(void) //运动中断
{
    MPU6050WriteReg(MPU6050_RA_MOT_THR, 0x03); //运动阈值
    MPU6050WriteReg(MPU6050_RA_MOT_DUR, 0x14); //检测时间20ms 单位1ms 寄存器0X20
}
void ZeroMotionInterrupt(void) //静止中断
{
    MPU6050WriteReg(MPU6050_RA_ZRMOT_THR, 0x20); //静止阈值
    MPU6050WriteReg(MPU6050_RA_ZRMOT_DUR, 0x20); //静止检测时间20ms 单位1ms 寄存器0X20
}

void MPU6050Init(void)
{
    int i = 0, j = 0;
    //在初始化之前要延时一段时间，若没有延时，则断电后再上电数据可能会出错
    for (i = 0; i < 1000; i++)
    {
        for (j = 0; j < 1000; j++)
        {
            ;
        }
    }
    MPU6050WriteReg(MPU6050_RA_PWR_MGMT_1, 0X80); //复位MPU6050
    usleep(20000);
    MPU6050WriteReg(MPU6050_RA_PWR_MGMT_1, 0X00);   //唤醒MPU6050
    MPU6050WriteReg(MPU6050_RA_INT_ENABLE, 0X00);   //关闭所有中断
    MPU6050WriteReg(MPU6050_RA_USER_CTRL, 0X00);    //I2C主模式关闭
    MPU6050WriteReg(MPU6050_RA_FIFO_EN, 0X00);      //关闭FIFO
    MPU6050WriteReg(MPU6050_RA_INT_PIN_CFG, 0X80);  //中断的逻辑电平模式,设置为0，中断信号为高电；设置为1，中断信号为低电平时。
    MotionInterrupt();                              //运动中断
    MPU6050WriteReg(MPU6050_RA_CONFIG, 0x04);       //配置外部引脚采样和DLPF数字低通滤波器
    MPU6050WriteReg(MPU6050_RA_ACCEL_CONFIG, 0x1C); //加速度传感器量程和高通滤波器配置
    MPU6050WriteReg(MPU6050_RA_INT_PIN_CFG, 0X1C);  //INT引脚低电平平时
    MPU6050WriteReg(MPU6050_RA_INT_ENABLE, 0x40);   //中断使能寄存器
}

uint8_t MPU6050ReadID(void)
{
    unsigned char Re = 0;
    MPU6050ReadData(MPU6050_RA_WHO_AM_I, &Re, 1); //读器件地址
    printf("MPU6050ReadID==0x%x\n", Re);
    if (Re != 0x68)
    {
        printf("MPU6050 dectected error!\r\n");
        return 0;
    }
    else
    {
        return 1;
    }
}

void GyroInit(void)
{
    GyroIOInit();
    MPU6050Init();
    osDelay(100);
}

void GyroReadData(void)
{
    short Accel[3];
    short Temp;
    short Gyro[3];
    if (MPU6050ReadID() == 0)
    {
        printf("MPU6050ReadID failed-----\n");
        //while(1);
        do
        {
            MPU6050Init();
            osDelay(100);
            MPU6050ReadID();
        } while (MPU6050ReadID() != 0);
    }
    MPU6050ReadAcc(Accel);
    MPU6050ReturnTemp(&Temp);
    MPU6050ReadGyro(Gyro);
    Gyro_Data.Temperature = Temp;
    Gyro_Data.Accel[0] = Accel[0];
    Gyro_Data.Accel[1] = Accel[1];
    Gyro_Data.Accel[2] = Accel[2];
    Gyro_Data.Gyro[0] = Gyro[0];
    Gyro_Data.Gyro[1] = Gyro[1];
    Gyro_Data.Gyro[2] = Gyro[2];
    usleep(50000);
}

void LEDD1StatusSet(Gyro_Status_ENUM status)
{
    if (status == ON)
        IoTGpioSetOutputVal(IOT_GPIO_IO_GPIO_7, IOT_GPIO_VALUE0); // 设置GPIO_7输出高低电平点亮灯
    if (status == OFF)
        IoTGpioSetOutputVal(IOT_GPIO_IO_GPIO_7, IOT_GPIO_VALUE1); // 设置GPIO_7输出高电平关闭灯
}

void LEDD2StatusSet(Gyro_Status_ENUM status)
{
    if (status == ON)
        IoTGpioSetOutputVal(IOT_GPIO_IO_GPIO_8, IOT_GPIO_VALUE0); // 设置GPIO_8输出低电平点亮灯
    if (status == OFF)
        IoTGpioSetOutputVal(IOT_GPIO_IO_GPIO_8, IOT_GPIO_VALUE1); // 设置GPIO_8输出高电平关闭灯
}
